#include "precompiled.h"
#pragma hdrstop

#include <float.h>

namespace inf {
    Angles Angles::ZERO( 0.0f, 0.0f, 0.0f );


    /*
    =================
    Angles::normalize_360

    returns angles normalized to the range [0 <= angle < 360]
    =================
    */
    Angles& Angles::normalize_360( void ) {
        int i;

        for ( i = 0; i < 3; i++ ) {
            if ( ( (*this)[i] >= 360.0f ) || ( (*this)[i] < 0.0f ) ) {
                (*this)[i] -= math::floor( (*this)[i] / 360.0f ) * 360.0f;

                if ( (*this)[i] >= 360.0f ) {
                    (*this)[i] -= 360.0f;
                }
                if ( (*this)[i] < 0.0f ) {
                    (*this)[i] += 360.0f;
                }
            }
        }

        return *this;
    }

    /*
    =================
    Angles::normalize_180

    returns angles normalized to the range [-180 < angle <= 180]
    =================
    */
    Angles& Angles::normalize_180( void ) {
        normalize_360();

        if ( pitch > 180.0f ) {
            pitch -= 360.0f;
        }

        if ( yaw > 180.0f ) {
            yaw -= 360.0f;
        }

        if ( roll > 180.0f ) {
            roll -= 360.0f;
        }
        return *this;
    }

    /*
    =================
    Angles::to_Vectors
    =================
    */
    void Angles::to_Vectors( Vec3 *forward, Vec3 *right, Vec3 *up ) const {
        float sr, sp, sy, cr, cp, cy;

        math::sincos( math::DEG2RAD( yaw ), sy, cy );
        math::sincos( math::DEG2RAD( pitch ), sp, cp );
        math::sincos( math::DEG2RAD( roll ), sr, cr );

        if ( forward ) {
            forward->set( cp * cy, cp * sy, -sp );
        }

        if ( right ) {
            right->set( -sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp );
        }

        if ( up ) {
            up->set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
        }
    }

    /*
    =================
    Angles::to_forward
    =================
    */
    Vec3 Angles::to_forward( void ) const {
        float sp, sy, cp, cy;

        math::sincos( math::DEG2RAD( yaw ), sy, cy );
        math::sincos( math::DEG2RAD( pitch ), sp, cp );

        return Vec3( cp * cy, cp * sy, -sp );
    }

    /*
    =================
    Angles::to_Quat
    =================
    */
    Quat Angles::to_Quat( void ) const {
        float sx, cx, sy, cy, sz, cz;
        float sxcy, cxcy, sxsy, cxsy;

        math::sincos( math::DEG2RAD( yaw ) * 0.5f, sz, cz );
        math::sincos( math::DEG2RAD( pitch ) * 0.5f, sy, cy );
        math::sincos( math::DEG2RAD( roll ) * 0.5f, sx, cx );

        sxcy = sx * cy;
        cxcy = cx * cy;
        sxsy = sx * sy;
        cxsy = cx * sy;

        return Quat( cxsy*sz - sxcy*cz, -cxsy*cz - sxcy*sz, sxsy*cz - cxcy*sz, cxcy*cz + sxsy*sz );
    }

    /*
    =================
    Angles::to_Rotation
    =================
    */
    Rotation Angles::to_Rotation( void ) const {
        Vec3 vec;
        float angle, w;
        float sx, cx, sy, cy, sz, cz;
        float sxcy, cxcy, sxsy, cxsy;

        if ( pitch == 0.0f ) {
            if ( yaw == 0.0f ) {
                return Rotation( Vec3::ORIGIN, Vec3( -1.0f, 0.0f, 0.0f ), roll );
            }
            if ( roll == 0.0f ) {
                return Rotation( Vec3::ORIGIN, Vec3( 0.0f, 0.0f, -1.0f ), yaw );
            }
        } else if ( yaw == 0.0f && roll == 0.0f ) {
            return Rotation( Vec3::ORIGIN, Vec3( 0.0f, -1.0f, 0.0f ), pitch );
        }

        math::sincos( math::DEG2RAD( yaw ) * 0.5f, sz, cz );
        math::sincos( math::DEG2RAD( pitch ) * 0.5f, sy, cy );
        math::sincos( math::DEG2RAD( roll ) * 0.5f, sx, cx );

        sxcy = sx * cy;
        cxcy = cx * cy;
        sxsy = sx * sy;
        cxsy = cx * sy;

        vec.x =  cxsy * sz - sxcy * cz;
        vec.y = -cxsy * cz - sxcy * sz;
        vec.z =  sxsy * cz - cxcy * sz;
        w =		 cxcy * cz + sxsy * sz;
        angle = math::acos( w );
        if ( angle == 0.0f ) {
            vec.set( 0.0f, 0.0f, 1.0f );
        } else {
            //vec *= (1.0f / sin( angle ));
            vec.normalize();
            vec.fix_degenerate_normal();
            angle *= 2.0f * math::M_RAD2DEG;
        }
        return Rotation( Vec3::ORIGIN, vec, angle );
    }

    /*
    =================
    Angles::to_Mat3

    Rotation matrices used:

    X =	??

    Y = ??

    Z = ??

    Rotation order: M = ?.?.?

    | cos(y) * cos(z)                               cos(y) * sin(z)                              -sin(y)         |
    M = | sin(x) * sin(y) * cos(z) + cos(x) * -sin(z)   sin(x) * sin(y) * sin(z) + cos(x) * cos(z)   sin(x) * cos(y) |
    | cos(x) * sin(y) * cos(z) + -sin(x) * -sin(z)  cos(x) * sin(y) * sin(z) + -sin(x) * cos(z)  cos(x) * cos(y) |

    pitch = rotation around y axis
    yaw = rotation around z axis
    roll = rotation around x axis

    =================
    */
    Mat3 Angles::to_Mat3( void ) const {
        Mat3 mat;
        float sr, sp, sy, cr, cp, cy;

        math::sincos( math::DEG2RAD( yaw ), sy, cy );
        math::sincos( math::DEG2RAD( pitch ), sp, cp );
        math::sincos( math::DEG2RAD( roll ), sr, cr );

        mat[ 0 ].set( cp * cy, cp * sy, -sp );
        mat[ 1 ].set( sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp );
        mat[ 2 ].set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );

        return mat;
    }

    /*
    =================
    Angles::to_Mat3_no_roll
    =================
    */
    void Angles::to_Mat3_no_roll( Mat3& mat ) const {
        float sp, sy, cp, cy;

        math::sincos( math::DEG2RAD( yaw ), sy, cy );
        math::sincos( math::DEG2RAD( pitch ), sp, cp );

        mat[ 0 ].set( cp * cy, cp * sy, -sp );
        mat[ 1 ].set( -sy, cy, 0.0f );
        mat[ 2 ].set( sp * cy, sp * sy, cp );
    }

    /*
    =================
    Angles::to_Mat4
    =================
    */
    Mat4 Angles::to_Mat4( void ) const {
        return to_Mat3().to_Mat4();
    }

    /*
    =================
    Angles::to_angular_velocity
    =================
    */
    Vec3 Angles::to_angular_velocity( void ) const {
        return Angles::to_Rotation().to_angular_velocity();
    }

    /*
    =============
    Angles::to_string
    =============
    */
    const char *Angles::to_string( int precision ) const {
        return float_array_to_string_ANSI( to_float_ptr(), get_dimension(), precision );
    }

    /*
    =================
    Angles::to_Mat3_Maya

    Rotation matrices used:

    | 1    0      0    |
    X =	| 0  cos(x) sin(x) |
    | 0 -sin(x) cos(x) |

    | cos(y) 0 -sin(y) |
    Y =	|   0    1    0    |
    | sin(y) 0  cos(y) |

    | cos(z)  sin(z) 0 |
    Z =	| -sin(z) cos(z) 0 |
    |   0       0    1 |

    Rotation order: M = X.Y.Z

    | cos(y) * cos(z)                               cos(y) * sin(z)                              -sin(y)         |
    M = | sin(x) * sin(y) * cos(z) + cos(x) * -sin(z)   sin(x) * sin(y) * sin(z) + cos(x) * cos(z)   sin(x) * cos(y) |
    | cos(x) * sin(y) * cos(z) + -sin(x) * -sin(z)  cos(x) * sin(y) * sin(z) + -sin(x) * cos(z)  cos(x) * cos(y) |

    pitch = rotation around x axis
    yaw = rotation around y axis
    roll = rotation around z axis

    =================
    */
    Mat3 Angles::to_Mat3_Maya( void ) const {
        Mat3 mat;
        float sr, sp, sy, cr, cp, cy;

        math::sincos( math::DEG2RAD( yaw ), sy, cy );
        math::sincos( math::DEG2RAD( pitch ), sp, cp );
        math::sincos( math::DEG2RAD( roll ), sr, cr );

        mat[ 0 ].set( cy * cr, cy * sr, -sy );
        mat[ 1 ].set( sp * sy * cr + cp * -sr, sp * sy * sr + cp * cr, sp * cy );
        mat[ 2 ].set( cp * sy * cr + -sp * -sr, cp * sy * sr + -sp * cr, cp * cy );

        return mat;
    }

}   // ::inf
